
#include "stdafx.h"
#include "pf_math.h"

#include "pf_debug.h"

/**
 * Rounds the given double to the next int. 
 */
int PF_Math::round(double v) {
    return (v-floor(v)<0.5 ? (int)floor(v) : (int)ceil(v));
}




/**
 * Save pow function
 */
double PF_Math::pow(double x, double y) {
	errno = 0;
	double ret = ::pow(x, y);
	if (errno==EDOM) {
        PF_DEBUG->print(PF_Debug::D_ERROR,
			"PF_Math::pow: EDOM in pow");
		ret = 0.0;
	}
	else if (errno==ERANGE) {
        PF_DEBUG->print(PF_Debug::D_WARNING,
			"PF_Math::pow: ERANGE in pow");
		ret = 0.0;
	}
	return ret;
}



/**
 * Converts radians to degrees.
 */
double PF_Math::rad2deg(double a) {
    return (a/(2.0*M_PI)*360.0);
}



/**
 * Converts degrees to radians.
 */
double PF_Math::deg2rad(double a) {
    return ((a/360.0)*(2.0*M_PI));
}



/**
 * Converts radians to gradians.
 */
double PF_Math::rad2gra(double a) {
    return (a/(2.0*M_PI)*400.0);
}



/**
 * Finds greatest common divider using Euclid's algorithm.
 */
int PF_Math::findGCD(int a, int b) {
    int rem;

    while (b!=0) {
        rem = a % b;
        a = b;
        b = rem;
    }

    return a;
}



/**
 * Tests if angle a is between a1 and a2. a, a1 and a2 must be in the 
 * range between 0 and 2*PI.
 * All angles in rad.
 *
 * @param reversed true for clockwise testing. false for ccw testing.
 * @return true if the angle a is between a1 and a2.
 */
bool PF_Math::isAngleBetween(double a,
                             double a1, double a2,
                             bool reversed) {

    bool ret = false;

    if (reversed) {
        double tmp = a1;
        a1 = a2;
        a2 = tmp;
    }

    if(a1>=a2-1.0e-12) {
        if(a>=a1-1.0e-12 || a<=a2+1.0e-12) {
            ret = true;
        }
    } else {
        if(a>=a1-1.0e-12 && a<=a2+1.0e-12) {
            ret = true;
        }
    }
    //PF_DEBUG->print("angle %f is %sbetween %f and %f",
    //                a, ret ? "" : "not ", a1, a2);
    return ret;
}



/**
 * Corrects the given angle to the range of 0-2*Pi.
 */
double PF_Math::correctAngle(double a) {
    while (a>2*M_PI)
        a-=2*M_PI;
    while (a<0)
        a+=2*M_PI;
    return a;
}



/**
 * @return The angle that needs to be added to a1 to reach a2.
 *         Always positive and less than 2*pi.
 */
double PF_Math::getAngleDifference(double a1, double a2) {
    double ret;

    if (a1>=a2) {
        a2+=2*M_PI;
    }
    ret = a2-a1;

    if (ret>=2*M_PI) {
        ret=0.0;
    }

    return ret;
}


/**
* Makes a text constructed with the given angle readable. Used
* for dimension texts and for mirroring texts.
*
* @param readable true: make angle readable, false: unreadable
* @param corrected Will point to true if the given angle was
*   corrected, false otherwise.
* 
 * @return The given angle or the given angle+PI, depending which on
 * is readable from the bottom or right.
 */
double PF_Math::makeAngleReadable(double angle, bool readable,
                                  bool* corrected) {

    double ret;

    bool cor = isAngleReadable(angle) ^ readable;

    // quadrant 1 & 4
    if (!cor) {
        ret = angle;
    }
    // quadrant 2 & 3
    else {
        ret = angle+M_PI;
    }

    if (corrected!=NULL) {
        *corrected = cor;
    }

    return ret;
}


/**
 * @return true: if the given angle is in a range that is readable 
 * for texts created with that angle.
 */
bool PF_Math::isAngleReadable(double angle) {
    if (angle>M_PI/2.0*3.0+0.001 ||
            angle<M_PI/2.0+0.001) {
        return true;
    } else {
        return false;
    }
}



/**
 * @param tol Tolerance in rad.
 * @retval true The two angles point in the same direction.
 */
bool PF_Math::isSameDirection(double dir1, double dir2, double tol) {
	double diff = fabs(dir1-dir2);
	if (diff<tol || diff>2*M_PI-tol) {
		//std::cout << "PF_Math::isSameDirection: " << dir1 << " and " << dir2 
		//	<< " point in the same direction" << "\n";
		return true;
	}
	else {
		//std::cout << "PF_Math::isSameDirection: " << dir1 << " and " << dir2 
		//	<< " don't point in the same direction" << "\n";
		return false;
	}
}


/**
 * Compares two double values with a tolerance.
 */
bool PF_Math::cmpDouble(double v1, double v2, double tol) {
    return (fabs(v2-v1)<tol);
}



/**
 * Evaluates a mathematical expression and returns the result.
 * If an error occured, the given default value 'def' will be returned.
 */
double PF_Math::eval(const QString& expr, double def) {

    bool ok;
    double res = PF_Math::eval(expr, &ok);

    if (!ok) {
        //std::cerr << "PF_Math::evaluate: Parse error at col "
        //<< ret << ": " << fp.ErrorMsg() << "\n";
        return def;
    }

    return res;
}


/**
 * Evaluates a mathematical expression and returns the result.
 * If an error occured, ok will be set to false (if ok isn't NULL).
 */
//double PF_Math::eval(const QString& expr, bool* ok);


/**
 * Converts a double into a string which is as short as possible
 *
 * @param value The double value
 * @param prec Precision e.g. a precision of 1 would mean that a 
 *     value of 2.12030 will be converted to "2.1". 2.000 is always just "2").
 */
QString PF_Math::doubleToString(double value, double prec) {
	if (prec<1.0e-12) {
		std::cerr << "PF_Math::doubleToString: invalid precision\n";
		return "";
	}

    QString ret;
    QString exaStr;
    int dotPos;
    int num = PF_Math::round(value / prec);

    exaStr = PF_Math::doubleToString(prec, 10);
    dotPos = exaStr.indexOf('.');

    if (dotPos==-1) {
        ret.sprintf("%d", PF_Math::round(num*prec));
    } else {
        int digits = exaStr.length() - dotPos - 1;
        ret = PF_Math::doubleToString(num*prec, digits);
    }

    return ret;
}




/**
 * Converts a double into a string which is as short as possible.
 *
 * @param value The double value
 * @param prec Precision
 */
QString PF_Math::doubleToString(double value, int prec) {
    QString valStr;

    valStr.setNum(value, 'f', prec);

    if(valStr.contains('.')) {
        // Remove zeros at the end:
        while (valStr.at(valStr.length()-1)=='0') {
            valStr.truncate(valStr.length()-1);
        }

        if(valStr.at(valStr.length()-1)=='.') {
            valStr.truncate(valStr.length()-1);
        }
    }

    return valStr;
}



/**
 * Performs some testing for the math class.
 */
void PF_Math::test() {
    QString s;
    double v;

    std::cout << "PF_Math::test: doubleToString:\n";

    v = 0.1;
    s = PF_Math::doubleToString(v, 0.1);
    assert(s=="0.1");
    s = PF_Math::doubleToString(v, 0.01);
    assert(s=="0.1");
    s = PF_Math::doubleToString(v, 0.0);
    assert(s=="0");

    v = 0.01;
    s = PF_Math::doubleToString(v, 0.1);
    assert(s=="0");
    s = PF_Math::doubleToString(v, 0.01);
    assert(s=="0.01");
    s = PF_Math::doubleToString(v, 0.0);
    assert(s=="0");

    v = 0.001;
    s = PF_Math::doubleToString(v, 0.1);
    assert(s=="0");
    s = PF_Math::doubleToString(v, 0.01);
    assert(s=="0");
    s = PF_Math::doubleToString(v, 0.001);
    assert(s=="0.001");
    s = PF_Math::doubleToString(v, 0.0);
    assert(s=="0");

    std::cout << "PF_Math::test: complete\n";
}

